#ifndef __SV2_CAMERA_READ__
#define __SV2_CAMERA_READ__

#include "sms_core.h"
#include "sv2_camera_input.h"
#include <opencv2/opencv.hpp>
#include <string>
#include <chrono>
#include <mutex>
#include <queue>
#include <iostream>
#include <chrono>
#include <thread>
#include <unordered_map>

/*
依赖安装
sudo apt install libopencv-dev
sudo apt install libopencv-contrib-dev

*/

namespace sv2
{

  class CameraCppNode : public sms::BaseNode
  {
  public:
    CameraCppNode(
        std::string job_name,
        std::string param_file,
        std::string ip = "127.0.0.1",
        int port = 9094) : sms::BaseNode("CameraCppNode", job_name, param_file, ip, port),
                           _calib_pub("/" + job_name + "/sensor/calibration_info", "sensor_msgs::CameraCalibration"),
                           _image_pub("/" + job_name + "/sensor/image_raw", "sensor_msgs::CompressedImage")
    {

      // 读取节点参数
      this->camera_type = this->get_param("camera_type", "V4L2CAM");
      this->camera_ip = this->get_param("camera_ip", "192.168.2.64");
      this->camera_id = this->get_param("camera_id", 0);
      this->image_width = this->get_param("width", 640);
      this->image_height = this->get_param("height", 480);
      this->fps = this->get_param("fps", 30);
      this->frame_id = this->get_param("frame_id", "camera");
      this->camera_matrix = this->get_param("camera_matrix", {7.9379415710551370e+02, 0, 2.9783879354295328e+02, 0,
                                                              7.9491985564466654e+02, 3.0942416136837386e+02, 0, 0, 1});
      this->distortion_coefficients = this->get_param("distortion_coefficients", {2.0950200339181715e-01, -1.1587468096518483e+00,
                                                                                  5.5342063671841328e-03, 2.2214393775334758e-04, 1.7127431916651392e+00});
      this->rectification = this->get_param("rectification", {0, 0, 0, 0, 0, 0, 0, 0, 0});
      this->projection = this->get_param("projection", {7.9379415710551370e+02, 0, 2.9783879354295328e+02, 0, 0,
                                                        7.9491985564466654e+02, 3.0942416136837386e+02, 0, 0, 0, 1, 0});

      
      logger.info("frame_id: " + this->frame_id);
      logger.info("camera_type: " + this->camera_type);
      logger.info("image_width: " + std::to_string(static_cast<int>(this->image_width)));
      logger.info("image_height: " + std::to_string(static_cast<int>(this->image_height)));
      logger.info("fps: " + std::to_string(static_cast<double>(this->fps)));
      logger.info("camera_matrix: " + this->camera_matrix.dump());
      logger.info("distortion_coefficients: " + this->distortion_coefficients.dump());

      // 打开摄像头
      this->cap->setWH(this->image_width, this->image_height);
      this->cap->setFps(this->fps);
      this->cap->setIp(this->camera_ip);
      this->cap->open(this->getCameraType(this->camera_type), this->camera_id); // CameraID 0
    }
    ~CameraCppNode()
    {
    }
    void run();
    CameraType getCameraType(const std::string &camera_string);

    cv::Mat img_;
    nlohmann::json camera_matrix;
    nlohmann::json distortion_coefficients;
    nlohmann::json rectification;
    nlohmann::json projection;
    std::string frame_id;
    std::string camera_type;
    std::string camera_ip;
    int image_width;
    int image_height;
    int camera_id;
    double fps;
    Camera *cap = new Camera;

  private:
    // 发布话题
    sms::Publisher _image_pub;
    sms::Publisher _calib_pub;
  };

}
#endif
